package com.evolved.automata.experimental.bots;


import java.lang.reflect.Method;



import com.evolved.automata.experimental.tools.*;


public class SRV1Robot implements AbstractRobot{
	private static String s_TestOutputFolder ="E:\\AI_system\\blog\\agent_tests\\"; 
	private static String s_DeltaPictureName="image_delta_%1$s.ppm";
	private static int s_Counter=0;
	
	int m_Width=320;
	int m_Height=240;
	
	public static final int LEFT=0;
	public  static final int RIGHT=1;
	public  static final int BACK=2;
	public static  final int FORWARD=3;
	public static  final int FORWARD_LEFT=4;
	public static  final int FORWARD_RIGHT=5;
	public static  final int BACKWARD_LEFT=6;
	public static  final int BACKWARD_RIGHT=7;
	
	
	public static  final int FAST_LEFT=8;
	public static  final int FAST_RIGHT=9;
	public static  final int FAST_BACK=10;
	public static  final int FAST_FORWARD=11;
	public static  final int FAST_FORWARD_LEFT=12;
	public static  final int FAST_FORWARD_RIGHT=13;
	public static  final int FAST_BACKWARD_LEFT=14;
	public static  final int FAST_BACKWARD_RIGHT=15;

	
	public static  final int VERY_FASTLEFT=16;
	public static  final int VERY_FASTRIGHT=17;
	public static  final int VERY_FASTBACK=18;
	public static  final int VERY_FASTFORWARD=19;
	public static  final int VERY_FASTFORWARD_LEFT=20;
	public static  final int VERY_FASTFORWARD_RIGHT=21;
	public static  final int VERY_FASTBACKWARD_LEFT=22;
	public static  final int VERY_FASTBACKWARD_RIGHT=23;

	public static final int STOP=24;

	
	
	int m_ActionResponseTimeoutMilli=100; // This is the maximum time to wait for for a motor enable response
	int m_LaserOnTimeoutMilli=100;
	int m_LaserStateDelayMilli=300;
	int m_MovementStateDelayMilli=300;
	int m_MotionFrameDelayMilli=20;
	int m_MinimumMotionBlurMass=300; // this is a coarse measurement of the amount of change between two consecutive
									 // frames to be considered enough motion to be significant
	int m_MinimumMotionStreamSize=20; // This is the minimum mass of the motion blur blobs to considered a real movement
	
	public final int m_DistanceConversion=925;
	private Dimension[] m_LaserSearchBounds;
	int m_LaserOnDelayMilli=100;
	int m_LaserOffDelayMilli=100;
	int m_MotionStepMilli=150;
	int m_ColorDistanceThreshold=20;
	int[] m_CurrentImage;
	
	private int[] m_ImageDifference;
	
	public final int m_Normal=0;
	public final int m_Fast=1;
	public final int m_VeryFast=2;
	public int[] m_LaserImageDifference=null;
	public int m_CurrentDistance;
	
	
	private SRV1SocketInterface srv1RawInterface;
	
	public SRV1Robot(String hostIP, int hostPort)
	{
		srv1RawInterface = new SRV1SocketInterface(hostIP,hostPort,3000);
		m_CurrentImage=new int[m_Width*m_Height*3];
		m_LaserImageDifference=new int[m_Width*m_Height*3];
		m_LaserSearchBounds=new Dimension[]{new Dimension(0,40), new Dimension(m_Width-1,m_Height-55)};
	}
	
	public SRV1Robot()
	{
		srv1RawInterface = new SRV1SocketInterface();
		m_CurrentImage=new int[m_Width*m_Height*3];
		m_LaserImageDifference=new int[m_Width*m_Height*3];
		m_LaserSearchBounds=new Dimension[]{new Dimension(0,40), new Dimension(m_Width-1,m_Height-55)};
	}
	
	
	public void setLaserSearchBounds(Dimension botLeft, Dimension topRight)
	{
		m_LaserSearchBounds=new Dimension[]{botLeft, topRight};
	}
	
	
	public void Pause(int waitMilli)
	{
		try
		{
			Thread.currentThread().sleep(waitMilli);
		}
		catch (Exception e)
		{
			
		}
	}
	
	private int convertGetLinearDistance(int pixelDistance)
	{
		return m_DistanceConversion/pixelDistance;
	}
	
	
	public int GetForwardDistance()
	{
		int len=3*m_Width*m_Height;
		boolean response=true;
		int[] laserOffImage, laserOnImage,colorDifference=null;
		laserOffImage=srv1RawInterface.GetCurrentImage();
		
		int estimatedDistance=-1;
		if (laserOffImage!=null)
		{
			srv1RawInterface.TurnOnLasers();
			Pause(m_LaserOnDelayMilli);
			laserOnImage=srv1RawInterface.GetCurrentImage();
			response=response&&(laserOnImage!=null);
			srv1RawInterface.TurnOffLasers();
			Pause(m_LaserOffDelayMilli);
			if (response)
			{
				m_LaserImageDifference=colorDifference=BasicTools.ImageSubtractDifference(laserOnImage, laserOffImage,m_Width,m_Height);

				BasicTools.FilterBrightnessPercent(m_LaserImageDifference, m_Width, m_Height, .4);
				int[] croppedForLaser;
				int croppedWidth=Math.abs(m_LaserSearchBounds[0].width-m_LaserSearchBounds[1].width)+1;
				int croppedHeight=Math.abs(m_LaserSearchBounds[0].height-m_LaserSearchBounds[1].height)+1;
				croppedForLaser=BasicTools.ExtractColors(
						m_LaserImageDifference,
						m_Width, 
						m_Height,
						m_LaserSearchBounds[0].width,
						m_LaserSearchBounds[0].height,
						m_LaserSearchBounds[1].width,
						m_LaserSearchBounds[1].height);
				
				
				BlobInfo[] laserBlobs=BasicTools.GetBlobs(croppedForLaser, croppedWidth, croppedHeight, .5, 1, 40, 2);
				
				
				if (laserBlobs.length>1)
				{
					double ax=0;
					int length=0;
					for (BlobInfo laserBlob: laserBlobs)
					{
						length++;
						ax=(ax==0)?laserBlob.getCentroid().width:(ax*(length-1)+laserBlob.getCentroid().width)/length;
					}
					
					int leftLength=0;
					int rightLength=0;
					BlobInfo leftMetaBlobCentroid=null;
					BlobInfo rightMetaBlobCentroid=null;
					
					for (BlobInfo laserBlob: laserBlobs)
					{
						if (laserBlob.getCentroid().width<=ax)
						{
							leftLength++;
							
							if (leftLength==1)
							{
								leftMetaBlobCentroid= (BlobInfo)laserBlob.clone();
							}
							else
							{
								leftMetaBlobCentroid.addAllPoints(laserBlob);
							}
							
						}
						else
						{
							rightLength++;
							if (rightLength==1)
								rightMetaBlobCentroid= (BlobInfo)laserBlob.clone();
							else
								rightMetaBlobCentroid.addAllPoints(laserBlob);
						}
					}
					if ((rightMetaBlobCentroid!=null)&&(leftMetaBlobCentroid!=null))
					{
						double laserSpotAngle=BasicTools.angle(leftMetaBlobCentroid.getCentroid(), rightMetaBlobCentroid.getCentroid());
						if (Math.abs(laserSpotAngle)<Math.PI/6)
						{
							//estimatedDistance=convertGetLinearDistance((int)leftMetaBlobCentroid.distance(rightMetaBlobCentroid));
							estimatedDistance=convertGetLinearDistance(Math.abs(leftMetaBlobCentroid.getCentroid().width-rightMetaBlobCentroid.getCentroid().width));
							
						}
					}
				}
				
			}
			
		}
		
		if (estimatedDistance==-1)
		{
			
			BasicTools.SavePPM(s_TestOutputFolder + String.format(s_DeltaPictureName, s_Counter++), colorDifference, m_Width, m_Height);
		}
		
		return m_CurrentDistance=estimatedDistance; 
	}
	
	public int GetCurrentDistance()
	{
		return m_CurrentDistance;
	}
	
	public boolean MoveForward(int speed)
	{
		return srv1RawInterface.MoveRobot(speed, speed, m_MotionStepMilli);
	}
	public boolean MoveBackward(int speed)
	{
		return srv1RawInterface.MoveRobot(-speed, -speed, m_MotionStepMilli);
	}
	public boolean MoveLeft(int speed)
	{
		return srv1RawInterface.MoveRobot(-speed, speed, m_MotionStepMilli);
	}
	public boolean MoveRight(int speed)
	{
		return srv1RawInterface.MoveRobot(speed, -speed, m_MotionStepMilli);
	}
	public boolean MoveForwardLeft(int speed)
	{
		return srv1RawInterface.MoveRobot(0, speed, m_MotionStepMilli);
	}
	public boolean MoveForwardRight(int speed)
	{
		return srv1RawInterface.MoveRobot(speed, 0, m_MotionStepMilli);
	}
	
	public boolean MoveBackLeft(int speed)
	{
		return srv1RawInterface.MoveRobot(0, -speed, m_MotionStepMilli);
	}
	public boolean MoveBackRight(int speed)
	{
		return srv1RawInterface.MoveRobot(-speed, 0, m_MotionStepMilli);
	}
	public boolean Stop()
	{
		return true;
	}
	
	public int[] GetLaserDifference()
	{
		return m_LaserImageDifference;
	}
	
	public boolean ForwardCollisionDetect()
	{
		return false;
	}
	public boolean BackwardCollisionDetect()
	{
		return false;
	}
	
	public int[] GetImageRawRGB()
	{
		return srv1RawInterface.GetCurrentImage();
	}
	
	public int[] GetCurrentImage()
	{
		return m_CurrentImage;
	}
	
	public int[] GetImageDifference()
	{
		return m_ImageDifference;
	}
	
	
	public int GetImageWidth()
	{
		return m_Width;
	}
	public int GetImageHeight()
	{
		return m_Height;
	}
	
	public Method[] GetImplementedMethods()
	{
		return null;
	}
	
	public void SetMotionStepSize(int stepMilli)
	{
		m_MotionStepMilli=stepMilli;
	}
	
	public boolean MoveRobotTotal(int motionType, int power)
	{
		boolean output;
		int[] startImage,afterImage;
		startImage=GetImageRawRGB();
		output=MoveSimple(motionType,power);
		afterImage=m_CurrentImage=GetImageRawRGB();
		m_ImageDifference=BasicTools.ImageSubtractDifference(afterImage, startImage,  m_Width, m_Height);
		
		return output;
	}
	
	public boolean MoveSimple(int motionType, int power)
	{
		switch (motionType)
		{
			case LEFT:
				
				return MoveLeft(power);
			case RIGHT:
				
				return MoveRight(power);
			case FORWARD:
				return MoveForward(power);
				
			case BACK:
				return MoveBackward(power);
				
			case FORWARD_LEFT:
				return MoveForwardLeft(power);
				
			case FORWARD_RIGHT:
				return MoveForwardRight(power);
				
			case BACKWARD_LEFT:
				return MoveBackLeft(power);
				
			case BACKWARD_RIGHT:
				return MoveBackRight(power);
				
			case STOP:
				return Stop();
					
		}
		return false;
	}
	
	
}
